using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Storage;
using Microsoft.Xna.Framework.Content;

using BriansClimb.Climber;
using BriansClimb.GameObjects;
using BriansClimb.DrawableGameObjects.ClimbingHolds;

using FarseerGames.FarseerPhysics;
using FarseerGames.FarseerPhysics.Dynamics;
using FarseerGames.FarseerPhysics.Collisions;
using FarseerGames.FarseerPhysics.Mathematics;

namespace BriansClimb.Climber
{
    /// <summary>
    /// This is the climber that the player controls when playing the game.
    /// </summary>
    public class Climber : Microsoft.Xna.Framework.DrawableGameComponent
    {
        protected Game briansClimb;
        protected PhysicsSimulator physicsSimulator;
        protected BodyPartInfoHandler bpih;        

        //!The list of body parts in the climber
        protected List<BodyPart> bodyPartsList = new List<BodyPart>();        
        //!The list of climbing holds for the wall that the climber is on
        protected List<ClimbingHold> wallList = new List<ClimbingHold>();
        //!The list of contact objects the climber has
        protected List<ContactObject> contactObjectsList = new List<ContactObject>();
        //!The list of bad contact objects that will be removed
        protected List<ContactObject> badContacts = new List<ContactObject>();
        
        protected BodyPart movingBodyPart;


        protected int bodyWeight, limbWeight, extremityWeight;
        protected int bodyWidth, bodyHeight, armWidth, armHeight, legWidth, legHeight;
        //!Dimensions of the climber's head
        protected int headDims;
        protected float climberFlexibility;

        protected int climberFriction;
        //!Indicates first update run of climber
        protected bool first = true;
        protected bool gravityToggle = false;
        protected bool hasLetGo = false;
        protected bool hasntBeenPlaced = true;

        protected KeyboardState thisKeyboardState, lastKeyboardState;
        protected MouseState thisState, lastState;
        protected GamePadState thisGamepadState, lastGamepadState;
        protected Geom pickedGeom;
        protected bool isGeomClicked;

        //the following are the body parts that compose the climber, including the floor, walls, and ceilings
        protected BodyPart headPart, bodyPart;
        protected BodyPart rightShoulderPart, rightForearmPart, leftShoulderPart, leftForearmPart;
        protected BodyPart rightThighPart, rightCalfPart, leftThighPart, leftCalfPart;
        protected BodyPart rightHandPart, leftHandPart, rightFootPart, leftFootPart;
        protected BodyPart floor, ceiling, leftWall, rightWall;
        protected BodyPart rightKneePart, leftKneePart;
        protected BodyPart neckPart;
        protected BodyPart leftArmPadPart, rightArmPadPart;


        //The following are the pin joints that pin body parts together.
        protected PinJoint rightElbowJoint;
        protected PinJoint rightWristJoint, rightWristJoint2, leftWristJoint, leftWristJoint2;
        protected PinJoint neckJoint2, neckJoint3, neckJoint4, neckJoint5;
        protected PinJoint leftPadJoint1, leftPadJoint2, leftPadJoint3, leftPadJoint4;
        protected PinJoint rightPadJoint1, rightPadJoint2, rightPadJoint3, rightPadJoint4;

        //The following are slider joints that pin body parts together but allow some range of motion between them.
        protected SliderJoint rightHipSlider, leftHipSlider;
        protected SliderJoint rightKneeSlider1, rightKneeSlider2, leftKneeSlider1, leftKneeSlider2, rightAnkleSlider, leftAnkleSlider;
        protected SliderJoint rightShoudlerSlider1, rightShoulderSlider2, leftShoulderSlider1, leftShoulderSlider2;

        //The following are angle limit joints that attatch between two bodies and restrict their angles of separation.
        protected AngleLimitJoint rightShoulderAngleLimitJoint, leftShoulderAngleLimitJoint, leftThighAngleLimitJoint, rightThighAngleLimitJoint;
        protected AngleLimitJoint rightWristAngleLimitJoint, leftWristAngleLimitJoint, rightAnkleAngleLimitJoint, leftAnkleAngleLimitJoint;
        protected AngleLimitJoint neckAngleLimitJoint2, neckAngleLimitJoint3;
        protected AngleLimitJoint rightElbowAngleLimitJoint, leftElbowAngleLimitJoint;
        protected AngleLimitJoint rightKneeAngleLimitJoint2, rightKneeAngleLimitJoint3;
        protected AngleLimitJoint leftKneeAngleLimitJoint2, leftKneeAngleLimitJoint3;

        //The following are strings used for the paths of the textures
        protected String headTexturePath, bodyTexturePath;
        protected String rightShoulderTexturePath, rightForearmTexturePath, leftShoulderTexturePath, leftForearmTexturePath;
        protected String rightThighTexturePath, rightCalfTexturePath, leftThighTexturePath, leftCalfTexturePath;
        protected String rightHandTexturePath, leftHandTexturePath, rightFootTexturePath, leftFootTexturePath;        

        //The following are lists to keep track of joints, body parts, and texture strings.
        protected List<PinJoint> jointList, removeTheseJoints;
        protected List<AngleLimitJoint> angleLimitJointList, removeTheseAngleJoints;
        protected List<BodyPart> hands, removeTheseHands;
        protected List<String> texturePathList;
        

        protected bool leftShoulderButtonPressed = false;
        protected bool lastLeftShoulderButtonPressed = false;

        SpriteBatch spriteBatch;
        
        /// <summary>
        /// Climber constructor.
        /// </summary>
        /// <param name="game">The master game object.</param>
        /// <param name="bpihIn">BodyPartInfoHandler in, used to pass wall lists to the climber.</param>
        public Climber(Game game, BodyPartInfoHandler bpihIn)
            : base(game)
        {
            jointList = new List<PinJoint>();
            removeTheseJoints = new List<PinJoint>();
            angleLimitJointList = new List<AngleLimitJoint>();
            removeTheseAngleJoints = new List<AngleLimitJoint>();
            hands = new List<BodyPart>();
            removeTheseHands = new List<BodyPart>();
            texturePathList = new List<string>();
            
            briansClimb = game;
            bpih = bpihIn;
            
            spriteBatch = (SpriteBatch)Game.Services.GetService(typeof(SpriteBatch));
            initializeBodyParts();
            initializeTexturePaths();
        }
                
        /// <summary>
        /// Sets the climber's stats.
        /// </summary>
        /// <param name="height">Height of the climber.  Depricated.</param>
        /// <param name="weight">Weight of the climber.</param>
        /// <param name="flex">Flexibility parameter of the climber.</param>
        /// <param name="strength">Strength parameter of the climber.  Depricated.</param>
        /// <param name="friction">Friction parameter of the climber.  Depricated.</param>
        /// <param name="physicsSimulator">Physics Simulator that the climber will be added to.</param>
        /// <param name="game">Master game object</param>
        /// <param name="initialPosition">Initial position of the climber.</param>
        public void setStats(int height, int weight, int flex, int strength, float friction, PhysicsSimulator physicsSimulator, Game game, Microsoft.Xna.Framework.Vector2 initialPosition)
        {
            createBody(height, weight, flex, strength, physicsSimulator, initialPosition);
            this.climberFriction = 100;
            this.physicsSimulator = physicsSimulator;
        }

        /// <summary>
        /// Sets the climber's friction.
        /// </summary>
        /// <param name="frictionIn">Friction to set the climber</param>
        public void setClimberFriction(int frictionIn)
        {
            climberFriction = frictionIn;
            leftHandPart.HandFriction = climberFriction;
            rightHandPart.HandFriction = climberFriction;                        
        }

        /// <summary>
        /// Sets the climber's flexibility.
        /// </summary>
        /// <param name="flexibilityIn">Flexibility to set the climber</param>
        public void setClimberFlexibility(float flexibilityIn)
        {
            climberFlexibility = flexibilityIn;
            leftThighAngleLimitJoint.UpperLimit = climberFlexibility;
            //right thigh's flexbility must be reversed
            rightThighAngleLimitJoint.LowerLimit = climberFlexibility * -1;
        }

        /// <summary>
        /// Whether the climber has let go of both holds.
        /// </summary>
        public bool HasLetGo
        {
            get { return hasLetGo; }
            set { hasLetGo = value; }
        }

        /// <summary>
        /// Initializes all of the body parts.
        /// </summary>
        private void initializeBodyParts()
        {
            rightFootPart = new BodyPart(briansClimb);
            leftFootPart = new BodyPart(briansClimb);
            rightCalfPart = new BodyPart(briansClimb);
            leftCalfPart = new BodyPart(briansClimb);
            rightThighPart = new BodyPart(briansClimb);
            leftThighPart = new BodyPart(briansClimb);
            bodyPart = new BodyPart(briansClimb);
            rightHandPart = new BodyPart(briansClimb);
            leftHandPart = new BodyPart(briansClimb);
            rightForearmPart = new BodyPart(briansClimb);
            leftForearmPart = new BodyPart(briansClimb);
            rightShoulderPart = new BodyPart(briansClimb);
            leftShoulderPart = new BodyPart(briansClimb);
            headPart = new BodyPart(briansClimb);
            floor = new BodyPart(briansClimb);
            ceiling = new BodyPart(briansClimb);
            leftWall = new BodyPart(briansClimb);
            rightWall = new BodyPart(briansClimb);
            rightKneePart = new BodyPart(briansClimb);
            leftKneePart = new BodyPart(briansClimb);
            neckPart = new BodyPart(briansClimb);
            leftArmPadPart = new BodyPart(briansClimb);
            rightArmPadPart = new BodyPart(briansClimb);
        }

        /// <summary>
        /// Initializes the paths of the textures so they can be concatenated with the individual paths later.
        /// </summary>
        private void initializeTexturePaths()
        {
            headTexturePath = ".\\Textures\\headTexture";
            bodyTexturePath = ".\\Textures\\bodyTexture";
            rightShoulderTexturePath = ".\\Textures\\rightShoulderTexture";
            leftShoulderTexturePath = ".\\Textures\\leftShoulderTexture";
            rightForearmTexturePath = ".\\Textures\\rightForearmTexture";
            leftForearmTexturePath = ".\\Textures\\leftForearmTexture";
            rightThighTexturePath = ".\\Textures\\rightThighTexture";
            leftThighTexturePath = ".\\Textures\\leftThighTexture";
            rightCalfTexturePath = ".\\Textures\\rightCalfTexture";
            leftCalfTexturePath = ".\\Textures\\leftCalfTexture";
            rightHandTexturePath = ".\\Textures\\rightHandTexture";
            leftHandTexturePath = ".\\Textures\\leftHandTexture";
            rightFootTexturePath = ".\\Textures\\rightFootTexture";
            leftFootTexturePath = ".\\Textures\\leftFootTexture";
        }

        /// <summary>
        /// Creates the climber body.
        /// </summary>
        /// <param name="height">Height of the body</param>
        /// <param name="weight">Weight of the body</param>
        /// <param name="flex">Flexbility parameter of the body.</param>
        /// <param name="strength">Strength parameter of the body.</param>
        /// <param name="physicsSimulator">Master physics simulator.</param>
        /// <param name="headVectorIn">Position to place the head in; all other body parts will be drawn relative to this.</param>
        private void createBody(int height, int weight, int flex, int strength, PhysicsSimulator physicsSimulator, Microsoft.Xna.Framework.Vector2 headVectorIn)
        {
            //Creates the farseer vector based in the input xna vector.  This is used for head position
            FarseerGames.FarseerPhysics.Mathematics.Vector2 headPosition = new FarseerGames.FarseerPhysics.Mathematics.Vector2(headVectorIn.X, headVectorIn.Y);

            //Sets height and weight values based on stats.  depricated.
            setHeightValues(height);
            setWeightValues(weight);

            /*The next block of code creates all of the body
             * parts used by the climber, positions them, and then creates the joints
             * between these body parts.
             */

            //Create head, neck, and body.
            headPart.makeBodyPart(headDims, headDims, extremityWeight, briansClimb.Content.Load<Texture2D>(headTexturePath), physicsSimulator, headPosition, "head", 1);
            neckPart.makeBodyPart(20, 20, extremityWeight, briansClimb.Content.Load<Texture2D>(".\\Textures\\neckTexture"), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(headPosition.X, headPosition.Y+25), "neck");
            neckPart.getGeom().CollisionEnabled = false;
            bodyPart.makeBodyPart(bodyWidth, bodyHeight, bodyWeight, briansClimb.Content.Load<Texture2D>(bodyTexturePath), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(headPosition.X, headPosition.Y + 78), "body");

            //Create the forearms, arm pads, and upper arms.
            leftShoulderPart.makeBodyPart(armWidth, armHeight, limbWeight, briansClimb.Content.Load<Texture2D>(leftShoulderTexturePath), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(headPosition.X - armWidth - 8, headPosition.Y + 10 + headDims / 2+2), "leftShoulder");
            leftShoulderPart.getGeom().CollisionEnabled = false;
            rightShoulderPart.makeBodyPart(armWidth, armHeight, limbWeight, briansClimb.Content.Load<Texture2D>(rightShoulderTexturePath), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(headPosition.X + headDims - 12, headPosition.Y + 10 + headDims / 2+2), "rightShoulder");
            rightShoulderPart.getGeom().CollisionEnabled = false;
            leftArmPadPart.makeBodyPart(25, 25, extremityWeight, briansClimb.Content.Load<Texture2D>(".\\Textures\\padTexture"), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(leftShoulderPart.getPosition().X-armWidth+15, leftShoulderPart.getPosition().Y), "leftArmPad");
            rightArmPadPart.makeBodyPart(25, 25, extremityWeight, briansClimb.Content.Load<Texture2D>(".\\Textures\\padTexture"), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(rightShoulderPart.getPosition().X+armWidth/2, rightShoulderPart.getPosition().Y), "rightArmPad");
            leftForearmPart.makeBodyPart(armWidth, armHeight, limbWeight, briansClimb.Content.Load<Texture2D>(leftForearmTexturePath), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(leftShoulderPart.getPosition().X - armWidth - 3, headPosition.Y + 10 + headDims / 2+2), "leftForearm");
            leftForearmPart.getGeom().CollisionEnabled = false;
            rightForearmPart.makeBodyPart(armWidth, armHeight, limbWeight, briansClimb.Content.Load<Texture2D>(rightForearmTexturePath), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(rightShoulderPart.getPosition().X + armWidth + 3, headPosition.Y + 10 + headDims / 2+2), "rightForearm");
            rightForearmPart.getGeom().CollisionEnabled = false;
            
            //Create the hands
            leftHandPart.makeBodyPart(10, 10, extremityWeight, briansClimb.Content.Load<Texture2D>(".\\Textures\\leftHandTexture"), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(leftForearmPart.getPosition().X - 20, headPosition.Y + 10 + headDims / 2+2), "leftHand");
            rightHandPart.makeBodyPart(10, 10, extremityWeight, briansClimb.Content.Load<Texture2D>(".\\Textures\\rightHandTexture"), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(rightForearmPart.getPosition().X + armWidth - 7, headPosition.Y + 10 + headDims / 2+1), "rightHand");

            //Create the thighs and calves.  The collision on legs are disabled because we were having issues with legs getting stuck on one another
            rightThighPart.makeBodyPart(legWidth, legHeight, limbWeight, briansClimb.Content.Load<Texture2D>(rightThighTexturePath), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(bodyPart.getPosition().X + bodyWidth / 2 -4, bodyPart.getPosition().Y + bodyHeight / 2 + 16), "rightThigh");
            rightThighPart.getGeom().CollisionEnabled = false;
            leftThighPart.makeBodyPart(legWidth, legHeight, limbWeight, briansClimb.Content.Load<Texture2D>(leftThighTexturePath), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(bodyPart.getPosition().X - bodyWidth / 2 + 4, bodyPart.getPosition().Y + bodyHeight / 2 + 16), "leftThigh");
            leftThighPart.getGeom().CollisionEnabled = false;
            leftKneePart.makeBodyPart(10, 10, extremityWeight, briansClimb.Content.Load<Texture2D>(".\\Textures\\rightHandTexture"), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(leftThighPart.getPosition().X - legWidth / 2 + 2, rightThighPart.getPosition().Y + legHeight - 7), "knee");
            leftKneePart.getGeom().CollisionGroup = 2;
            leftKneePart.getGeom().CollisionEnabled = false;
            leftCalfPart.makeBodyPart(legWidth, legHeight, limbWeight, briansClimb.Content.Load<Texture2D>(leftCalfTexturePath), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(leftThighPart.getPosition().X - legWidth / 2 + 2, leftThighPart.getPosition().Y + legHeight + 15), "leftCalf");
            leftCalfPart.getGeom().CollisionEnabled = false;
            rightKneePart.makeBodyPart(10, 10, extremityWeight, briansClimb.Content.Load<Texture2D>(".\\Textures\\rightHandTexture"), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(rightThighPart.getPosition().X - legWidth / 2 + 2, rightThighPart.getPosition().Y + legHeight - 7), "knee");
            rightKneePart.getGeom().CollisionGroup = 2;
            rightKneePart.getGeom().CollisionEnabled = false;
            rightCalfPart.makeBodyPart(legWidth, legHeight, limbWeight, briansClimb.Content.Load<Texture2D>(rightCalfTexturePath), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(rightThighPart.getPosition().X - legWidth / 2 + 2, rightThighPart.getPosition().Y + legHeight + 15), "rightCalf");
            rightCalfPart.getGeom().CollisionEnabled = false;

            //Create the feet
            leftFootPart.makeBodyPart(15, 5, limbWeight, briansClimb.Content.Load<Texture2D>(".\\Textures\\rightFootTexture"), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(leftCalfPart.getPosition().X - 4, leftCalfPart.getPosition().Y + legHeight - 7), "leftFoot");
            rightFootPart.makeBodyPart(15, 5, limbWeight, briansClimb.Content.Load<Texture2D>(".\\Textures\\leftFootTexture"), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(rightCalfPart.getPosition().X + 4, rightCalfPart.getPosition().Y + legHeight - 7), "rightFoot");

            /*Set the friction for the hands and feet.  Feet can grab anything so they are set to max.
             * hand values are set to their proper values later on.
             */

            leftHandPart.setIsHand(true);
            leftHandPart.HandFriction = 25;
            rightHandPart.setIsHand(true);
            rightHandPart.HandFriction = 25;
            leftFootPart.setIsHand(true);
            leftFootPart.HandFriction = 25;
            rightFootPart.setIsHand(true);
            rightFootPart.HandFriction = 25;

            //Adds the hands and feet to the CollisionText method so their collisions can be properly resolved.
            leftHandPart.getGeom().Collision += CollisionTest;
            rightHandPart.getGeom().Collision += CollisionTest;
            rightFootPart.getGeom().Collision += CollisionTest;
            leftFootPart.getGeom().Collision += CollisionTest;

            //Adds the hands, feet, and knees to the hands list.
            hands.Add(leftHandPart);
            hands.Add(rightHandPart);
            hands.Add(leftFootPart);
            hands.Add(rightFootPart);
            hands.Add(rightKneePart);
            hands.Add(leftKneePart);

            //Creates the floor, ceiling, and walls.
            floor.makeBodyPart(1280, 100, 999999999, briansClimb.Content.Load<Texture2D>(".\\Textures\\floorTexture"), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(640, 994), "floor");
            floor.getGeom().collisionEnabled = true;
            floor.getBody().isStatic = true;
            ceiling.makeBodyPart(1280, 100, 10000000, briansClimb.Content.Load<Texture2D>(".\\Textures\\floorTexture"), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(640, -50), "wall");
            ceiling.getGeom().collisionEnabled = true;
            ceiling.getBody().isStatic = true;
            leftWall.makeBodyPart(100, 1280, 999999999, briansClimb.Content.Load<Texture2D>(".\\Textures\\wallTexture"), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(-50, 400), "wall");
            leftWall.getGeom().collisionEnabled = true;
            leftWall.getBody().isStatic = true;
            rightWall.makeBodyPart(100, 1280, 999999999, briansClimb.Content.Load<Texture2D>(".\\Textures\\wallTexture"), physicsSimulator, new FarseerGames.FarseerPhysics.Mathematics.Vector2(1330, 400), "ceiling");
            rightWall.getGeom().collisionEnabled = true;
            rightWall.getBody().isStatic = true;

            addPartsToList();
            
            /*The following segment of code creates the joints for the climbers.  Oftentimes I had to 
             * create more than one joint for each corresponding real life joint in order to keep the
             * bodies joined by the joints more closely bound.
             */

            //Create neck joints
            neckJoint2 = JointFactory.Instance.CreatePinJoint(physicsSimulator, neckPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-neckPart.getTexture().Width/2f, 0), headPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, 0));
            neckJoint4 = JointFactory.Instance.CreatePinJoint(physicsSimulator, neckPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(neckPart.getTexture().Width / 2f, 0), headPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, 0));
            neckJoint3 = JointFactory.Instance.CreatePinJoint(physicsSimulator, bodyPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-bodyPart.getTexture().Width / 2f, 0), neckPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(neckPart.getTexture().Width / 2f, 0));
            neckJoint5 = JointFactory.Instance.CreatePinJoint(physicsSimulator, bodyPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(bodyPart.getTexture().Width/2f, 0), neckPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-neckPart.getTexture().Width / 2f, 0));
            jointList.Add(neckJoint2);
            jointList.Add(neckJoint3);
            jointList.Add(neckJoint4);
            jointList.Add(neckJoint5);            
            neckAngleLimitJoint2 = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, headPart.getBody(), neckPart.getBody(), 0f, 0f);
            neckAngleLimitJoint3 = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, neckPart.getBody(), bodyPart.getBody(), 0f, 0f);
            angleLimitJointList.Add(neckAngleLimitJoint2);
            angleLimitJointList.Add(neckAngleLimitJoint3);

            //Create elbow joints
            leftPadJoint1 = JointFactory.Instance.CreatePinJoint(physicsSimulator, leftArmPadPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, 0), leftForearmPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(leftForearmPart.getTexture().Width/2f,leftForearmPart.getTexture().Height/2f));
            leftPadJoint2 = JointFactory.Instance.CreatePinJoint(physicsSimulator, leftArmPadPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, 0), leftForearmPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(leftForearmPart.getTexture().Width / 2f, -leftForearmPart.getTexture().Height / 2f));
            leftPadJoint3 = JointFactory.Instance.CreatePinJoint(physicsSimulator, leftArmPadPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, 0), leftShoulderPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-leftForearmPart.getTexture().Width / 2f, -leftForearmPart.getTexture().Height / 2f));
            leftPadJoint4 = JointFactory.Instance.CreatePinJoint(physicsSimulator, leftArmPadPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, 0), leftShoulderPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-leftForearmPart.getTexture().Width / 2f, leftForearmPart.getTexture().Height / 2f));
            jointList.Add(leftPadJoint1);
            jointList.Add(leftPadJoint2);
            jointList.Add(leftPadJoint3);
            jointList.Add(leftPadJoint4);

            rightPadJoint1 = JointFactory.Instance.CreatePinJoint(physicsSimulator, rightArmPadPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, 0), rightForearmPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-rightForearmPart.getTexture().Width / 2f, rightForearmPart.getTexture().Height / 2f));
            rightPadJoint2 = JointFactory.Instance.CreatePinJoint(physicsSimulator, rightArmPadPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, 0), rightForearmPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-rightForearmPart.getTexture().Width / 2f, -rightForearmPart.getTexture().Height / 2f));
            rightPadJoint3 = JointFactory.Instance.CreatePinJoint(physicsSimulator, rightArmPadPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, 0), rightShoulderPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(rightForearmPart.getTexture().Width / 2f, -rightForearmPart.getTexture().Height / 2f));
            rightPadJoint4 = JointFactory.Instance.CreatePinJoint(physicsSimulator, rightArmPadPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, 0), rightShoulderPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(rightForearmPart.getTexture().Width / 2f, rightForearmPart.getTexture().Height / 2f));
            jointList.Add(rightPadJoint1);
            jointList.Add(rightPadJoint2);
            jointList.Add(rightPadJoint3);
            jointList.Add(rightPadJoint4);

            //Create shoulder slider joints
            rightShoudlerSlider1 = JointFactory.Instance.CreateSliderJoint(physicsSimulator, bodyPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(bodyPart.getTexture().Width / 2f, -bodyPart.getTexture().Height / 2f), rightShoulderPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-rightShoulderPart.getTexture().Width / 2f + 1, -rightShoulderPart.getTexture().Height / 2f), -10, 5);
            rightShoulderSlider2 = JointFactory.Instance.CreateSliderJoint(physicsSimulator, bodyPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(bodyPart.getTexture().Width / 2f, -bodyPart.getTexture().Height / 2f), rightShoulderPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-rightShoulderPart.getTexture().Width / 2f - 1, rightShoulderPart.getTexture().Height / 2f), -10, 5);            
            leftShoulderSlider1 = JointFactory.Instance.CreateSliderJoint(physicsSimulator, bodyPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-bodyPart.getTexture().Width / 2f, -bodyPart.getTexture().Height / 2f), leftShoulderPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(leftShoulderPart.getTexture().Width / 2f + 1, -leftShoulderPart.getTexture().Height / 2f),-10,5);
            leftShoulderSlider2 = JointFactory.Instance.CreateSliderJoint(physicsSimulator, bodyPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-bodyPart.getTexture().Width / 2f, -bodyPart.getTexture().Height / 2f), leftShoulderPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(leftShoulderPart.getTexture().Width / 2f - 1, leftShoulderPart.getTexture().Height / 2f), -10, 5);

           
            rightElbowJoint = JointFactory.Instance.CreatePinJoint(physicsSimulator, rightShoulderPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(rightShoulderPart.getTexture().Width / 2f, 0), rightForearmPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-rightForearmPart.getTexture().Width / 2f, 0));
            jointList.Add(rightElbowJoint);
            
            //Creates the wrist joints
            rightWristJoint = JointFactory.Instance.CreatePinJoint(physicsSimulator, rightForearmPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(rightForearmPart.getTexture().Width / 2f, rightForearmPart.getTexture().Height/2f), rightHandPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-rightHandPart.getTexture().Width / 2f, -rightHandPart.getTexture().Height/2f));
            rightWristJoint2 = JointFactory.Instance.CreatePinJoint(physicsSimulator, rightForearmPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(rightForearmPart.getTexture().Width / 2f, -rightForearmPart.getTexture().Height / 2f), rightHandPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-rightHandPart.getTexture().Width / 2f, rightHandPart.getTexture().Height / 2f));
            leftWristJoint = JointFactory.Instance.CreatePinJoint(physicsSimulator, leftForearmPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-leftForearmPart.getTexture().Width / 2f, 0), leftHandPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(leftHandPart.getTexture().Width / 2f, 0));
            leftWristJoint2 = JointFactory.Instance.CreatePinJoint(physicsSimulator, leftForearmPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-leftForearmPart.getTexture().Width / 2f, 0), leftHandPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(leftHandPart.getTexture().Width / 2f, 0));

            jointList.Add(rightWristJoint);
            jointList.Add(rightWristJoint2);
            jointList.Add(leftWristJoint);
            jointList.Add(leftWristJoint2);

            //Creates the angle joints for the shoulders and elbows
            rightShoulderAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, bodyPart.getBody(), rightShoulderPart.getBody(), -2f, 2f);
            angleLimitJointList.Add(rightShoulderAngleLimitJoint);
            leftShoulderAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, bodyPart.getBody(), leftShoulderPart.getBody(), -2f, 2f);
            angleLimitJointList.Add(leftShoulderAngleLimitJoint);
            
            rightElbowAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, rightShoulderPart.getBody(), rightForearmPart.getBody(), -1.5f, 1.5f);
            leftElbowAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, leftShoulderPart.getBody(), leftForearmPart.getBody(), -1.5f, 1.5f);            
            angleLimitJointList.Add(rightElbowAngleLimitJoint);
            angleLimitJointList.Add(leftElbowAngleLimitJoint);

            //Creates the hip joints and angle limit joints
            rightHipSlider = JointFactory.Instance.CreateSliderJoint(physicsSimulator, bodyPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(bodyPart.getTexture().Width/2f, bodyPart.getTexture().Height/2f), rightThighPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -rightThighPart.getTexture().Height/2f), 0,0);
            leftHipSlider = JointFactory.Instance.CreateSliderJoint(physicsSimulator, bodyPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-bodyPart.getTexture().Width / 2f, bodyPart.getTexture().Height / 2f), leftThighPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -leftThighPart.getTexture().Height / 2f),0,0);
            rightThighAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, bodyPart.getBody(), rightThighPart.getBody(), -2.5f, 0f);
            leftThighAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, bodyPart.getBody(), leftThighPart.getBody(), 0f, 2.5f);
            angleLimitJointList.Add(rightThighAngleLimitJoint);
            angleLimitJointList.Add(leftThighAngleLimitJoint);

            //Creates the knee joints and angle limit joints
            rightKneeSlider1 = JointFactory.Instance.CreateSliderJoint(physicsSimulator, rightThighPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, rightThighPart.getTexture().Height / 2f), rightKneePart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -rightKneePart.getTexture().Height / 2f), 0, 0);
            rightKneeSlider2 = JointFactory.Instance.CreateSliderJoint(physicsSimulator, rightKneePart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, rightKneePart.getTexture().Height / 2f), rightCalfPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -rightCalfPart.getTexture().Height / 2f), 0,0); 
            leftKneeSlider1 = JointFactory.Instance.CreateSliderJoint(physicsSimulator, leftThighPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, leftThighPart.getTexture().Height / 2f), leftKneePart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -leftKneePart.getTexture().Height / 2f),0,0);
            leftKneeSlider2 = JointFactory.Instance.CreateSliderJoint(physicsSimulator, leftKneePart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, leftKneePart.getTexture().Height / 2f), leftCalfPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -leftCalfPart.getTexture().Height / 2f), 0, 0);
            rightAnkleSlider = JointFactory.Instance.CreateSliderJoint(physicsSimulator, rightCalfPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, rightCalfPart.getTexture().Height / 2f), rightFootPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -rightFootPart.getTexture().Height / 2f),0,0);
            leftAnkleSlider = JointFactory.Instance.CreateSliderJoint(physicsSimulator, leftCalfPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, leftCalfPart.getTexture().Height / 2f), leftFootPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -leftFootPart.getTexture().Height / 2f),0,0);
            rightKneeAngleLimitJoint2 = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, rightThighPart.getBody(), rightKneePart.getBody(), 0f, 1f);
            rightKneeAngleLimitJoint3 = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, rightKneePart.getBody(), rightCalfPart.getBody(), 0f, 1f);
            angleLimitJointList.Add(rightKneeAngleLimitJoint2);
            angleLimitJointList.Add(rightKneeAngleLimitJoint3);
            leftKneeAngleLimitJoint2 = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, leftThighPart.getBody(), leftKneePart.getBody(), -1f, 0f);
            leftKneeAngleLimitJoint3 = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, leftKneePart.getBody(), leftCalfPart.getBody(), -1f, 0f);
            angleLimitJointList.Add(leftKneeAngleLimitJoint2);
            angleLimitJointList.Add(leftKneeAngleLimitJoint3);

            //Creates the ankle and wrist joints & angle limit joints
            rightAnkleAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, rightCalfPart.getBody(), rightFootPart.getBody(), -.3f, .3f);
            leftAnkleAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, leftCalfPart.getBody(), leftFootPart.getBody(), -.3f, .3f);
            angleLimitJointList.Add(rightAnkleAngleLimitJoint);
            angleLimitJointList.Add(leftAnkleAngleLimitJoint);
            rightWristAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, rightForearmPart.getBody(), rightHandPart.getBody(), -.3f, .3f);
            leftWristAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, leftForearmPart.getBody(), leftHandPart.getBody(), -.3f, .3f);
            angleLimitJointList.Add(rightWristAngleLimitJoint);
            angleLimitJointList.Add(leftWristAngleLimitJoint);

            //Changes each pinjoint softness, doing this resulted in less climber 'jitter'
            foreach (PinJoint pj in jointList)
            {
                pj.Softness = .05f;
            }
            //Changed the drag value of each body part, doing this made it easier to control the climber.
            foreach (BodyPart bp in bodyPartsList)
            {
                bp.getBody().LinearDragCoefficient = 1.55f;
            }

            
        }

        /// <summary>
        /// Adds each Body Part to the bodyPartsList.
        /// </summary>
        private void addPartsToList()
        {
            bodyPartsList.Add(rightFootPart);
            bodyPartsList.Add(leftFootPart);
            bodyPartsList.Add(rightCalfPart);
            bodyPartsList.Add(leftCalfPart);
            bodyPartsList.Add(rightThighPart);
            bodyPartsList.Add(leftThighPart);
            bodyPartsList.Add(bodyPart);
            bodyPartsList.Add(headPart);
            bodyPartsList.Add(leftShoulderPart);
            bodyPartsList.Add(rightShoulderPart);
            bodyPartsList.Add(leftForearmPart);
            bodyPartsList.Add(rightForearmPart);
            bodyPartsList.Add(leftHandPart);
            bodyPartsList.Add(rightHandPart);
            bodyPartsList.Add(rightKneePart);
            bodyPartsList.Add(leftKneePart);
            bodyPartsList.Add(neckPart);
            bodyPartsList.Add(leftArmPadPart);
            bodyPartsList.Add(rightArmPadPart);
            
                        
            /* Added these parts to the same collision group so they would not 
             * collide with ecah other.  This fixed several issues we had with
             * controls.
             */

            rightHandPart.Geom.CollisionGroup = 5;
            leftHandPart.Geom.CollisionGroup = 5;
            rightShoulderPart.Geom.CollisionGroup = 5;
            leftShoulderPart.Geom.CollisionGroup = 5;
            bodyPart.Geom.CollisionGroup = 5;
            rightThighPart.Geom.CollisionGroup = 5;
            leftThighPart.Geom.CollisionGroup = 5;
        }



        /// <summary>
        /// Sets the pixel height and width values for the arms, legs, and torso.
        /// </summary>
        /// <param name="heightValueIn">The paramter used to determine which pixel values to assign.</param>
        private void setHeightValues(int heightValueIn)
        {
            //TODO:  Add in texture name concatenations.
            if (heightValueIn == -2)
            {
                bodyHeight = 85;
                armWidth = 20;
                legHeight = 20;
                headDims = 40;
                concatenateTexturePaths(heightValueIn.ToString());
            }
            else if (heightValueIn == -1)
            {
                bodyHeight = 95;
                armWidth = 25;
                legHeight = 25;
                headDims = 45;
                concatenateTexturePaths(heightValueIn.ToString());
            }
            else if (heightValueIn == 0)
            {
                bodyHeight = 105;
                armWidth = 30;
                legHeight = 30;
                headDims = 50;
                concatenateTexturePaths(heightValueIn.ToString());
            }
            else if (heightValueIn == 1)
            {
                bodyHeight = 115;
                armWidth = 35;
                legHeight = 35;
                headDims = 55;
                concatenateTexturePaths(heightValueIn.ToString());
            }
            else if (heightValueIn == 2)
            {
                bodyHeight = 125;
                armWidth = 40;
                legHeight = 40;
                headDims = 60;
                concatenateTexturePaths(heightValueIn.ToString());
            }
            armHeight = 5;
            legWidth = 5;
            bodyWidth = 40;
        }


        /// <summary>
        /// Calculates values for body weight, limb weight, and extremity weight.
        /// </summary>
        /// <param name="weight">Weight of the climber.</param>
        private void setWeightValues(int weight)
        {
            bodyWeight = 3;
            limbWeight = 3;
            extremityWeight = 3;
        }

        /// <summary>
        /// Adds the height designator to the end of the file name to load different textures
        /// </summary>
        /// <param name="appendThis">The string to append to the end of the filename.</param>
        private void concatenateTexturePaths(String appendThis)
        {
            headTexturePath = headTexturePath + appendThis;
            bodyTexturePath = bodyTexturePath + appendThis;
            rightShoulderTexturePath = rightShoulderTexturePath + appendThis;
            rightForearmTexturePath = rightForearmTexturePath + appendThis;
            leftShoulderTexturePath = leftShoulderTexturePath + appendThis;
            leftForearmTexturePath = leftForearmTexturePath + appendThis;
            rightThighTexturePath = rightThighTexturePath + appendThis;
            rightCalfTexturePath = rightCalfTexturePath + appendThis;
            leftThighTexturePath = leftThighTexturePath + appendThis;
            leftCalfTexturePath = leftCalfTexturePath + appendThis;
            rightHandTexturePath = rightHandTexturePath + appendThis;
            leftHandTexturePath = leftHandTexturePath + appendThis;
            rightFootTexturePath = rightFootTexturePath + appendThis;
            leftFootTexturePath = leftFootTexturePath + appendThis;
        }



        /// <summary>
        /// Allows the game component to perform any initialization it needs to before starting
        /// to run.  This is where it can query for any required services and load content.
        /// </summary>
        public override void Initialize()
        {
            // TODO: Add your initialization code here
            base.Initialize();
        }        

        /// <summary>
        /// For testing
        /// </summary>
        private void junk()
        {
            Console.WriteLine("sadf");
        }

        FixedLinearSpring mousePickSpring;
        Body pickedBody, pickedBody2;
        BodyPart pickedBodyPart;
        Geom pickedHoldGeom;

        /// <summary>
        /// Handles moving the climber around.
        /// </summary>
        /// <param name="mouseInput">The current state of the mouse.</param>

        private void HandleInput(MouseState mouseInput)
        {   
            //Gets point based on mouse position.  Used for mouse controls.
            FarseerGames.FarseerPhysics.Mathematics.Vector2 point = new FarseerGames.FarseerPhysics.Mathematics.Vector2(mouseInput.X, mouseInput.Y);
            
            //Killed the mouse spring if the player dragged the selected body part too far above the body
            if (thisState.Y < bodyPart.getGeom().Position.Y - 275)
            {

                if (mousePickSpring != null)
                {
                    Console.WriteLine("Reach exceeded");
                    mousePickSpring.Dispose();
                    mousePickSpring = null;
                }
            }

            //Got the geom of the body part when the climber clicked on it.
            if (lastState.LeftButton == ButtonState.Released && thisState.LeftButton == ButtonState.Pressed)
            {
                pickedHoldGeom = physicsSimulator.Collide(point);
            }
            
            //Initialized and zeroed the force to be used to move bodies.
            FarseerGames.FarseerPhysics.Mathematics.Vector2 force = FarseerGames.FarseerPhysics.Mathematics.Vector2.Zero;
            force.Y = -force.Y;
            
            /*In the following methods, the knees are selected for 'picked body' when the user
             * selects the feet to move.  I did this because it was easier for the player to 
             * climb the wall when the forces were applied on the climber's knees rather than 
             * directly on his feet.  It also provided for a more realistic depiction of climbing,
             * as you move your knees to guide your feet instead of the other way around.
             */

            //Picking the right foot to move.
            if (((thisKeyboardState.IsKeyUp(Keys.Down)) && (lastKeyboardState.IsKeyDown(Keys.Down))) || ((thisGamepadState.IsButtonUp(Buttons.B)) && (lastGamepadState.IsButtonDown(Buttons.B))) || ((thisKeyboardState.IsKeyUp(Keys.OemSemicolon)) && (lastKeyboardState.IsKeyDown(Keys.OemSemicolon))))
            {
                pickedBody = rightKneePart.getBody();
                deselectHands(rightFootPart);
                pickedBody2 = rightFootPart.getBody();
                pickedBodyPart = rightFootPart;
                pickedGeom = rightFootPart.getGeom();
                pickedGeom.CollisionEnabled = true;                
            }
            //Picking the left foot to move.
            else if (((thisKeyboardState.IsKeyUp(Keys.Left)) && (lastKeyboardState.IsKeyDown(Keys.Left))) || ((thisGamepadState.IsButtonUp(Buttons.A)) && (lastGamepadState.IsButtonDown(Buttons.A)))||((thisKeyboardState.IsKeyUp(Keys.L)) && (lastKeyboardState.IsKeyDown(Keys.L))))
            {
                deselectHands(leftFootPart);
                pickedBody = leftKneePart.getBody();
                pickedBody2 = leftFootPart.getBody();
                pickedBodyPart = leftFootPart;
                pickedGeom = leftFootPart.getGeom();
                pickedGeom.CollisionEnabled = true;
            }
            //Picking the left hand to move.
            else if (((thisKeyboardState.IsKeyUp(Keys.Up)) && (lastKeyboardState.IsKeyDown(Keys.Up))) || ((thisGamepadState.IsButtonUp(Buttons.X)) && (lastGamepadState.IsButtonDown(Buttons.X))) || ((thisKeyboardState.IsKeyUp(Keys.O)) && (lastKeyboardState.IsKeyDown(Keys.O))))
            {
                deselectHands(leftHandPart);
                pickedBody = leftHandPart.getBody();
                pickedBody2 = pickedBody;
                pickedBodyPart = leftHandPart;
                pickedGeom = leftHandPart.getGeom();
                pickedGeom.CollisionEnabled = true;
                if (leftShoulderAngleLimitJoint.UpperLimit == leftShoulderAngleLimitJoint.LowerLimit)
                {
                    leftShoulderButtonPressed = true;
                    lastLeftShoulderButtonPressed = false;
                }
            }
            //Picking the right hand to move.
            else if (((thisKeyboardState.IsKeyUp(Keys.Right)) && (lastKeyboardState.IsKeyDown(Keys.Right))) || ((thisGamepadState.IsButtonUp(Buttons.Y)) && (lastGamepadState.IsButtonDown(Buttons.Y)))||((thisKeyboardState.IsKeyUp(Keys.P)) && (lastKeyboardState.IsKeyDown(Keys.P))))
            {
                deselectHands(rightHandPart);
                pickedBody = rightHandPart.getBody();
                pickedBody2 = pickedBody;
                pickedBodyPart = rightHandPart;
                pickedGeom = rightHandPart.getGeom();
                pickedGeom.CollisionEnabled = true;
                if (rightShoulderAngleLimitJoint.UpperLimit == rightShoulderAngleLimitJoint.LowerLimit)
                {
                    leftShoulderButtonPressed = true;
                    lastLeftShoulderButtonPressed = false;
                }
            }

            //Toggled the leftShoulderButtonPressed flag, was using for XBOX 360 controller control scheme.
            if ((thisGamepadState.IsButtonUp(Buttons.LeftShoulder)) && (lastGamepadState.IsButtonDown(Buttons.LeftShoulder)))
            {
                leftShoulderButtonPressed = !leftShoulderButtonPressed;
                Console.WriteLine(leftShoulderButtonPressed);
            }

            //Froze joints when the left shoulder was pressed.  Was using with controller control scheme.
            if ((leftShoulderButtonPressed)&&(!lastLeftShoulderButtonPressed))
            {
                if ((pickedBodyPart != null) && (pickedBodyPart.getName() == "leftHand"))
                {
                    float tempDiff = leftShoulderAngleLimitJoint.Difference;
                    physicsSimulator.Remove(leftShoulderAngleLimitJoint);
                    leftShoulderAngleLimitJoint.Dispose();
                    leftShoulderAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, bodyPart.getBody(), leftShoulderPart.getBody(), tempDiff, tempDiff);
                }
                if ((pickedBodyPart != null) && (pickedBodyPart.getName() == "rightHand"))
                {
                    float tempDiff = rightShoulderAngleLimitJoint.Difference;
                    physicsSimulator.Remove(rightShoulderAngleLimitJoint);
                    rightShoulderAngleLimitJoint.Dispose();
                    rightShoulderAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, bodyPart.getBody(), rightShoulderPart.getBody(), tempDiff, tempDiff);
                }
                if ((pickedBodyPart != null) && (pickedBodyPart.getName() == "leftFoot"))
                {
                    float tempDiff = leftThighAngleLimitJoint.Difference;
                    physicsSimulator.Remove(leftThighAngleLimitJoint);
                    leftThighAngleLimitJoint.Dispose();
                    leftThighAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, bodyPart.getBody(), leftThighPart.getBody(), tempDiff, tempDiff);
                }
                if ((pickedBodyPart != null) && (pickedBodyPart.getName() == "rightFoot"))
                {
                    float tempDiff = rightThighAngleLimitJoint.Difference;
                    physicsSimulator.Remove(rightThighAngleLimitJoint);
                    rightThighAngleLimitJoint.Dispose();
                    rightThighAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, bodyPart.getBody(), rightThighPart.getBody(), tempDiff, tempDiff);
                }
            }

            //Unfroze joints when using the controller and pressing the left shoulder button again.
            else if((!leftShoulderButtonPressed)&&(lastLeftShoulderButtonPressed))
            {                
                if ((pickedBodyPart != null) && (pickedBodyPart.getName() == "leftHand"))
                {
                    physicsSimulator.Remove(leftShoulderAngleLimitJoint);
                    leftShoulderAngleLimitJoint.Dispose();
                    leftShoulderAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, bodyPart.getBody(), leftShoulderPart.getBody(), -2f, 2f);   
                }
                if ((pickedBodyPart != null) && (pickedBodyPart.getName() == "rightHand"))
                {
                    physicsSimulator.Remove(rightShoulderAngleLimitJoint);
                    rightShoulderAngleLimitJoint.Dispose();
                    rightShoulderAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, bodyPart.getBody(), rightShoulderPart.getBody(), -2f, 2f);
                }
                if ((pickedBodyPart != null) && (pickedBodyPart.getName() == "leftFoot"))
                {
                    physicsSimulator.Remove(leftThighAngleLimitJoint);
                    leftThighAngleLimitJoint.Dispose();
                    leftThighAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, bodyPart.getBody(), leftThighPart.getBody(), -1f, 1.5f);
                }
                if ((pickedBodyPart != null) && (pickedBodyPart.getName() == "rightFoot"))
                {
                    physicsSimulator.Remove(rightThighAngleLimitJoint);
                    rightThighAngleLimitJoint.Dispose();
                    rightThighAngleLimitJoint = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, bodyPart.getBody(), rightThighPart.getBody(), -1.5f, 1f);
                }
            }
               

            //this works to freeze an angle limit joint.
            if ((Keyboard.GetState().IsKeyDown(Keys.OemOpenBrackets))&&(lastKeyboardState.IsKeyUp(Keys.OemOpenBrackets)))
            {
                Console.WriteLine(rightThighAngleLimitJoint.Difference);
                Console.WriteLine(rightKneeAngleLimitJoint2.Difference);
                Console.WriteLine(rightKneeAngleLimitJoint3.Difference);
            }
            
            //thumbstick up
            if (thisGamepadState.ThumbSticks.Left.Y > 0f)
            {
                force += new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, thisGamepadState.ThumbSticks.Left.Y * -9000f);
                Console.WriteLine(force);
                bodyPart.getBody().ApplyForce(force);
            }

            /* T and Space were our 'jump' up buttons that we used to simulat when a climber
             * reached for a climbing hold.  We created new slider joints for his hips because
             * we found that the climber could not pull his legs and body past his regular hip joints
             * to reach further.  Adding the slider joints gave the climber some slack between
             * his hips and his legs and allowed him to reach without being limited by his
             * hip joints.
             */

            //When Space or T is pressed, slider joints created to allow slack.
            if ((Keyboard.GetState().IsKeyDown(Keys.Space)) || (Keyboard.GetState().IsKeyDown(Keys.T)))
            {
                rightHipSlider.Dispose();
                leftHipSlider.Dispose();
                physicsSimulator.Remove(rightHipSlider);
                physicsSimulator.Remove(leftHipSlider);
                rightHipSlider = JointFactory.Instance.CreateSliderJoint(physicsSimulator, bodyPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(bodyPart.getTexture().Width / 2f, bodyPart.getTexture().Height / 2f), rightThighPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -rightThighPart.getTexture().Height / 2f), 0, 10);
                leftHipSlider = JointFactory.Instance.CreateSliderJoint(physicsSimulator, bodyPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-bodyPart.getTexture().Width / 2f, bodyPart.getTexture().Height / 2f), leftThighPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -leftThighPart.getTexture().Height / 2f), 0, 10);
                force += new FarseerGames.FarseerPhysics.Mathematics.Vector2((float)Math.Sin((double)bodyPart.Body.rotation)*18000, (float)Math.Cos((double)bodyPart.Body.rotation)*-18000);
                //force += new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -6000f);
                bodyPart.getBody().ApplyForce(force);
            }
            //When Space or T is released, hip joints returned to normal.
            if (((lastKeyboardState.IsKeyDown(Keys.Space))&&(thisKeyboardState.IsKeyUp(Keys.Space))) || ((lastKeyboardState.IsKeyDown(Keys.T))&&(thisKeyboardState.IsKeyUp(Keys.T))))
            {
                rightHipSlider.Dispose();
                leftHipSlider.Dispose();
                physicsSimulator.Remove(rightHipSlider);
                physicsSimulator.Remove(leftHipSlider);
                rightHipSlider = JointFactory.Instance.CreateSliderJoint(physicsSimulator, bodyPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(bodyPart.getTexture().Width / 2f, bodyPart.getTexture().Height / 2f), rightThighPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -rightThighPart.getTexture().Height / 2f), 0, 0);
                leftHipSlider = JointFactory.Instance.CreateSliderJoint(physicsSimulator, bodyPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(-bodyPart.getTexture().Width / 2f, bodyPart.getTexture().Height / 2f), leftThighPart.getBody(), new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -leftThighPart.getTexture().Height / 2f), 0, 0);
            }
            //thumbstick down
            if (thisGamepadState.ThumbSticks.Left.Y < 0f)
            {
                force += new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, thisGamepadState.ThumbSticks.Left.Y * -6000f);
                bodyPart.getBody().ApplyForce(force);
            }
            //thumbstick left
            if (thisGamepadState.ThumbSticks.Left.X > 0f)
            {
                force += new FarseerGames.FarseerPhysics.Mathematics.Vector2(thisGamepadState.ThumbSticks.Left.X * 6000f, 0);
                bodyPart.getBody().ApplyForce(force);
            }
            //thumbstick right
            if (thisGamepadState.ThumbSticks.Left.X < 0f)
            {
                force += new FarseerGames.FarseerPhysics.Mathematics.Vector2(thisGamepadState.ThumbSticks.Left.X * 6000f, 0);
                bodyPart.getBody().ApplyForce(force);
            }

            //Applies forces when using the controller.
            if ((thisGamepadState.ThumbSticks.Right.X != 0f) || (thisGamepadState.ThumbSticks.Right.Y != 0f))
            {
                if ((pickedBody == leftHandPart.getBody()) && (leftHandPart.getPosition().X < headPart.getPosition().X))
                    applyForces(pickedGeom, pickedBody, pickedBody2, true);   
                else if ((pickedBody == rightHandPart.getBody()) && (rightHandPart.getPosition().X > headPart.getPosition().X))
                    applyForces(pickedGeom, pickedBody, pickedBody2, true);   
                else if ((pickedBody == leftKneePart.getBody()) || (pickedBody == rightKneePart.getBody()))
                    applyForces(pickedGeom, pickedBody, pickedBody2, true);                
            }

            //Applies forces upward on the selected body part when using the keyboard.
            if (Keyboard.GetState().IsKeyDown(Keys.W) && lastKeyboardState.IsKeyDown(Keys.W))
            {
                if (pickedGeom != null)
                    applyForces(pickedGeom, pickedBody, pickedBody2, "E");
            }

            //Applies forces leftward on the selected body part when using the keyboard.
            if (Keyboard.GetState().IsKeyDown(Keys.S))
            {
                if (pickedGeom != null)
                    applyForces(pickedGeom, pickedBody, pickedBody2, "D");
            }

            //Applies forces down on the selected body part when using the keyboard.
            if (Keyboard.GetState().IsKeyDown(Keys.A))
            {
                if (pickedGeom != null)
                    applyForces(pickedGeom, pickedBody, pickedBody2, "S");
            }

            //Applies forces to the right on the selected body part when using the keyboard.
            if (thisKeyboardState.IsKeyDown(Keys.D))
            {
                if (pickedGeom != null)
                    applyForces(pickedGeom, pickedBody, pickedBody2, "F");
            }
            
            /*This was used for mouse controls, got the hold geom based on the mouse
             * position when clicked and applied a resultant force on the body that
             * was selected.
             */
            if (thisState.LeftButton == ButtonState.Pressed)
            {
                foreach (ClimbingHold ch in wallList)
                {
                    if ((thisState.X < ch.ClimbingHoldPosition.X + 50) && (thisState.X > ch.ClimbingHoldPosition.X - 50))
                        if ((thisState.Y < ch.ClimbingHoldPosition.Y + 50) && (thisState.Y > ch.ClimbingHoldPosition.Y - 50))
                            if (pickedGeom != null)
                            {
                                applyForces(pickedGeom, pickedBody2, pickedBody2);
                                if (ch.ClimbingHoldPosition.Y < pickedBody.position.Y)
                                {
                                    force += new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -20000f);
                                    bodyPart.getBody().ApplyForce(force);
                                }
                            }
                }
            }
            /* The following statements enabled and disabled hand/foot collision statused based
             * on if the 'apply force' buttons were being pushed.  Collision was turned off on
             * the body's geometry object when a force was being applied on it, and turned back
             * on when the force was no longer applying on it.
             */
            if (pickedGeom != null)
            {
                if (lastKeyboardState.IsKeyDown(Keys.D) || lastKeyboardState.IsKeyDown(Keys.S) || lastKeyboardState.IsKeyDown(Keys.A) || lastKeyboardState.IsKeyDown(Keys.W) )
                    if (thisKeyboardState.IsKeyUp(Keys.D) || thisKeyboardState.IsKeyUp(Keys.S) || thisKeyboardState.IsKeyUp(Keys.A) || thisKeyboardState.IsKeyUp(Keys.W))
                    {
                        pickedGeom.CollisionEnabled = true;
                    }
                if (thisKeyboardState.IsKeyDown(Keys.D) || thisKeyboardState.IsKeyDown(Keys.S) || thisKeyboardState.IsKeyDown(Keys.A) || thisKeyboardState.IsKeyDown(Keys.W))
                    if (lastKeyboardState.IsKeyUp(Keys.D) || lastKeyboardState.IsKeyUp(Keys.S) || lastKeyboardState.IsKeyUp(Keys.A) || lastKeyboardState.IsKeyUp(Keys.W))
                        pickedGeom.CollisionEnabled = false;
                if (thisKeyboardState.IsKeyDown(Keys.D) || thisKeyboardState.IsKeyDown(Keys.S) || thisKeyboardState.IsKeyDown(Keys.A) || thisKeyboardState.IsKeyDown(Keys.W))
                    if (lastKeyboardState.IsKeyDown(Keys.D) || lastKeyboardState.IsKeyDown(Keys.S) || lastKeyboardState.IsKeyDown(Keys.A) || lastKeyboardState.IsKeyDown(Keys.W))
                        pickedGeom.CollisionEnabled = false;
                if (thisKeyboardState.IsKeyDown(Keys.Space) && lastKeyboardState.IsKeyDown(Keys.Space))
                {
                    rightFootPart.getGeom().CollisionEnabled = false;
                    leftFootPart.getGeom().CollisionEnabled = false;
                }
                if ((thisState.LeftButton == ButtonState.Released) && (lastState.LeftButton == ButtonState.Pressed))
                    pickedGeom.CollisionEnabled = true;
                if ((thisState.LeftButton == ButtonState.Pressed) && (lastState.LeftButton == ButtonState.Pressed))
                    pickedGeom.CollisionEnabled = false;
                if ((thisState.LeftButton == ButtonState.Pressed) && (lastState.LeftButton == ButtonState.Released))
                    pickedGeom.CollisionEnabled = false;
                if((thisGamepadState.ThumbSticks.Right==new Microsoft.Xna.Framework.Vector2(0,0))&&(lastGamepadState.ThumbSticks.Right!=new Microsoft.Xna.Framework.Vector2(0,0)))
                    pickedGeom.CollisionEnabled=true;

            }

        }

        /// <summary>
        /// Applies forces based on keyboard input. 
        /// </summary>
        /// <param name="geomIn">Geometry object that is going to be coming into contact with a hold</param>
        /// <param name="bodyInOne">Body that has the force acted upon it.  If feet are selected, this will be a knee.</param>
        /// <param name="bodyInTwo">Secondary body, used if feet are selected. </param>
        /// <param name="keyString">Key that is pressed.</param>
        void applyForces(Geom geomIn, Body bodyInOne, Body bodyInTwo, String keyString)
        {
            FarseerGames.FarseerPhysics.Mathematics.Vector2 force = FarseerGames.FarseerPhysics.Mathematics.Vector2.Zero;
            force.Y = -force.Y;
            ContactObject removeMe = new ContactObject(briansClimb);

            if (bodyInOne != null)
            {
                geomIn.CollisionEnabled = false;
                if (bodyInTwo.IsStatic == true)
                    bodyInTwo.IsStatic = false;


                foreach (ContactObject co in contactObjectsList)
                {

                    if (bodyInTwo == co.contactHand.getBody())
                        removeMe = co;
                }
                if (removeMe.contactHold != null)
                {
                    removeMe.contactHold.IsBeingHeld = false;                    
                }
                contactObjectsList.Remove(removeMe);
            }

            switch (keyString)
            {
                case "F":
                    force += new FarseerGames.FarseerPhysics.Mathematics.Vector2(14000, 0);
                    break;
                case "S":
                    force += new FarseerGames.FarseerPhysics.Mathematics.Vector2(-14000, 0);
                    break;
                case "E":
                    force += new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -14000);
                    break;
                case "D":
                    force += new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, 14000);
                    break;
            }
            bodyInOne.ApplyForce(force);
        }

        /// <summary>
        /// Applies forces, used with mouse input.
        /// </summary>
        /// <param name="geomIn">Geometry object that is going to be coming into contact with a hold</param>
        /// <param name="bodyInOne">Body that will have a force acted upon it.  If selected body part is a foot, this will be a knee.</param>
        /// <param name="bodyInTwo">Secondary body part, used if selected part is a foot.</param>
        private void applyForces(Geom geomIn, Body bodyInOne, Body bodyInTwo)
        {
            FarseerGames.FarseerPhysics.Mathematics.Vector2 force = FarseerGames.FarseerPhysics.Mathematics.Vector2.Zero;
            force.Y = -force.Y;
            ContactObject removeMe = new ContactObject(briansClimb);

            if (bodyInOne != null)
            {
                geomIn.CollisionEnabled = false;
                if (bodyInTwo.IsStatic == true)
                    bodyInTwo.IsStatic = false;


                foreach (ContactObject co in contactObjectsList)
                {

                    if (bodyInTwo == co.contactHand.getBody())
                        removeMe = co;
                }
                if (removeMe.contactHold != null)
                {
                    removeMe.contactHold.IsBeingHeld = false;
                    //Console.WriteLine(removeMe.contactHold.IsBeingHeld);
                }
                contactObjectsList.Remove(removeMe);
            }
            if (pickedHoldGeom != null)
            {
                FarseerGames.FarseerPhysics.Mathematics.Vector2 point = new FarseerGames.FarseerPhysics.Mathematics.Vector2(thisState.X, thisState.Y);
                float fx = pickedHoldGeom.Position.X - (float)bodyInOne.Position.X;
                float fy = pickedHoldGeom.Position.Y - (float)bodyInOne.Position.Y;
                force += new FarseerGames.FarseerPhysics.Mathematics.Vector2(fx * 150, fy * 150);
                bodyInOne.IsStatic = false;
                bodyInOne.ApplyForce(force);
            }
        }

        /// <summary>
        /// Applies forces, used with controller input.
        /// </summary>
        /// <param name="geomIn">Geometry object that is going to be coming into contact with a hold</param>
        /// <param name="bodyInOne">Body that will have a force acted upon it.  If selected body part is a foot, this will be a knee.</param>
        /// <param name="bodyInTwo">Secondary body part, used if selected part is a foot.</param>
        /// <param name="trueIn">Used to indicate the overloaded method.</param>
    private void applyForces(Geom geomIn, Body bodyInOne, Body bodyInTwo, bool trueIn)
    {
        FarseerGames.FarseerPhysics.Mathematics.Vector2 force = FarseerGames.FarseerPhysics.Mathematics.Vector2.Zero;
        force.Y = -force.Y;
        ContactObject removeMe = new ContactObject(briansClimb);

        if (bodyInOne != null)
        {
            geomIn.CollisionEnabled = false;
            if (bodyInTwo.IsStatic == true)
                bodyInTwo.IsStatic = false;


            foreach (ContactObject co in contactObjectsList)
            {

                if (bodyInTwo == co.contactHand.getBody())
                    removeMe = co;
            }
            if (removeMe.contactHold != null)
            {
                removeMe.contactHold.IsBeingHeld = false;
                //Console.WriteLine(removeMe.contactHold.IsBeingHeld);
            }
            contactObjectsList.Remove(removeMe);
        }
        if (bodyInOne != null)
       {
           
            force += new FarseerGames.FarseerPhysics.Mathematics.Vector2(thisGamepadState.ThumbSticks.Right.X * 14000, thisGamepadState.ThumbSticks.Right.Y * -14000);
            bodyInOne.IsStatic = false;
            bodyInOne.ApplyForce(force);
        }
    }
        /// <summary>
        /// Deselects all of the hands and selects the input hand.
        /// </summary>
        /// <param name="bpIn">The hand to be selected.</param>

        private void deselectHands(BodyPart bpIn)
        {
            foreach (BodyPart bp in hands)
            {
                bp.IsSelected = false;
            }
            bpIn.IsSelected = true;
        }



        /// <summary>
        /// Calculates whether the hand has a high enough friction to stay on the hold.  Compares the
        /// hand geometry object with the hands list to get the correct body part and thus the correct
        /// friction, and compares the hold geometry object with the holds list to get the correct
        /// hold and thus the correct hold friction.
        /// </summary>
        /// <param name="handGeom">The geometry object of the hand.</param>
        /// <param name="holdGeom">The geometry object of the hold.</param>
        /// <returns>Wether the hand stays on the hold</returns>
        private bool frictionCalculation(Geom handGeom, Geom holdGeom)
        {
            //initializing hand and hold parts
            BodyPart hand = new BodyPart(briansClimb);
            ClimbingHold hold = new ClimbingHold(briansClimb);            
            
            foreach (BodyPart bp in hands)
                if (bp.getGeom() == handGeom)
                {
                    //gets the specific hand geom and body based on the input body
                    string name = bp.getName();
                    switch (name)
                    {
                        case "leftFoot":
                            hand = leftFootPart;
                            break;
                        case "rightFoot":
                            hand = rightFootPart;
                            break;
                        case "leftHand":
                            hand = leftHandPart;
                            break;
                        case "rightHand":
                            hand = rightHandPart;
                            break;
                    }
                }
            foreach (ClimbingHold ch in wallList)
            {
                //gets the specific hold geom and body based on the input body
                if((ch.ClimbingHoldPosition.X == holdGeom.Position.X)&&(ch.ClimbingHoldPosition.Y==holdGeom.Position.Y))
                {
                    hold = ch;                    
                }
            }
            
            //calculates if the climber can hold onto the hold.
            if (hand.HandFriction >= hold.HoldFriction)
            {
                //Successful hold.
                /* Changes the color speed of the hand based on the difference
                 * of the hand's friction and the hold's friction.
                 */

                hold.ColorSpeed = 10*(hand.HandFriction - hold.HoldFriction);
                
                /*Creates a new contact object containing the hand and the hold
                 * and adds it to the contactobject list if it is not already 
                 * there.
                 */

                ContactObject tempCO = new ContactObject(hold, hand, briansClimb);
                if (contactObjectsList.Count > 0)
                {
                    foreach (ContactObject co in contactObjectsList)
                    {
                        if (co.contactHold == hold)
                            co.contactHold.HoldCount = co.contactHold.HoldCount + 1;
                    }
                }

                //Flags the hand as holding and the hold as being held.
                if (!contactObjectsList.Contains(tempCO))
                {
                    tempCO.contactHand.IsHolding = true;
                    tempCO.contactHold.IsBeingHeld = true;
                    tempCO.contactHold.HeldBy = hand.getName();
                    contactObjectsList.Add(tempCO);
                }          
                
                
                return true;
            }

            else
            {                     
                //Unsuccesful hold
                return false;
            }
        }

        protected bool floorContact=false;

        /// <summary>
        /// Tracks collisions to check for hand/hold collision.
        /// </summary>
        /// <param name="geometry1">The first geometry object in the collision.</param>
        /// <param name="geometry2">The second geometry object in the collision.</param>
        /// <param name="contactList">The list of collisions.</param>
        /// <returns></returns>
        private bool CollisionTest(Geom geometry1, Geom geometry2, ContactList contactList)
        {
            //Checks if the climber has collided with the floor.
            if ((geometry1 == floor.getGeom()) || (geometry2 == floor.getGeom()))
                floorContact = true;

            //Checks collisions between hands and holds, runs friction calc if true to see if hold is successful.
            if ((geometry1.Body.IsHand) && (geometry2.Body.IsHold))
            {
                if (frictionCalculation(geometry1, geometry2))
                {
                    //Hold successful, sets hand to static.
                    geometry1.Body.IsStatic = true;
                    return true;
                }
                else
                {                
                    //Hold unsuccessful, sets hold to not static.
                    geometry1.Body.IsStatic = false;
                    return false;
                }

            
            }
            //Checks collisions between hands and holds, runs friction calc if true to see if hold is successful.
            else if ((geometry1.Body.IsHold) && (geometry2.Body.IsHand))
            {

                if (frictionCalculation(geometry2, geometry1))
                {
                    //Hold successful, sets hand to static.
                    geometry2.Body.IsStatic = true;
                    return true;
                }
                else
                {
                    //Hold unsuccessful, sets hold to not static.
                    geometry2.Body.IsStatic = false;
                    return false;
                }
            }

            //Returns a false on hands colliding with hands.  This fixed a game-crashing bug.
            else if ((geometry1.Body.IsHand) && (geometry2.Body.IsHand))
            {                
                return false;
            }
            
            //Disables collisions on disposed & removed objects.
            else if ((geometry1.IsDisposed) || (geometry2.IsDisposed) || (geometry1.isRemoved) || (geometry2.isRemoved))
            {            
                return false;
            }
            //Disables on collisions on everything else
            else
            {
                return true;
            }
        }

        
        /// <summary>
        /// Indicates the climber to let go with all hands.
        /// </summary>
        public void letGo()
        {
            foreach (ContactObject co in contactObjectsList)
            {
                co.contactHand.setIsHand(false);
                co.contactHand.getBody().IsStatic = false;
                co.contactHand.getBody().ApplyForce(new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -5000));
            }
        }
        /// <summary>
        /// Sets the climber's wall list to the given wall list.
        /// </summary>
        /// <param name="holdListIn">Wall list</param>
        public void setWallList(List<ClimbingHold> holdListIn)
        {
            wallList = holdListIn;                  
        }

        /// <summary>
        /// Goes through the ContactObjectList and checks the grip of each contact object.  Lets go if grip has run out.
        /// </summary>
        /// <param name="gameTime">Game Time for syncing.</param>
        private void checkGrip(GameTime gameTime)
        {            
            foreach(ContactObject co in contactObjectsList)
            {                
                //If the color on the contact hold is red, climber needs to let go of it
                if (co.contactHold.HoldIsRed)
                {
                    //Indicates that the hold is not being held if the hold does not have any more hands grabbing onto it
                    if (co.contactHold.HoldCount == 0)
                    {
                        co.contactHold.HoldIsRed = false;
                        co.contactHold.IsBeingHeld = false;
                        co.contactHold.ColorSpeed = 5;
                        co.contactHold.HeldBy = null;
                    }
                    //Removes the hand away from the hold, preps the badcontacts list
                    co.contactHand.getBody().IsStatic = false;
                    co.contactHand.getGeom().CollisionEnabled = false;
                    co.contactHand.getBody().ApplyForce(new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -5000));
                    co.contactHand.IsHolding = false;
                    badContacts.Add(co);
                    
                }
            }

            //Goes through the contacts in badContacts and removes them from the list of good contacts.
            foreach(ContactObject co in badContacts)
            {
                if(contactObjectsList.Contains(co))
                    contactObjectsList.Remove(co);                
            }
            badContacts.Clear();
         
                
        }
    
        /// <summary>
        /// Clears the climber's wall list.
        /// </summary>
        public void clearHoldList()
        {
            wallList.Clear();
        }



        /* The following method was a pain to get working.  I tried every trick I knew
         * to get the climber to reset successfully but nothing worked.
         */

        /// <summary>
        /// Resets the climber and recreates his body.
        /// </summary>
        /// <param name="height"></param>
        /// <param name="weight"></param>
        /// <param name="flex"></param>
        /// <param name="strength"></param>
        /// <param name="friction"></param>
        /// <param name="physicsSimulator"></param>
        /// <param name="game"></param>
        /// <param name="initialPosition"></param>
        public void reset(int height, int weight, int flex, int strength, float friction, PhysicsSimulator physicsSimulator, Game game, Microsoft.Xna.Framework.Vector2 initialPosition)
        {           
            foreach (BodyPart bp in bodyPartsList)
            {
                /* Disables every body part, disposes every body part & geom object, turns off collision
                 * on every geom object, removes every body and geom object from the physics simulator, and
                 * sets the geoms and bodies to null.
                 */

                bp.Enabled = false;
                bp.Body.enabled = false;
                bp.Geom.CollisionEnabled = false;
                bp.Geom.CollisionResponseEnabled = false;
                
                bp.Body.Dispose();
                bp.Geom.Dispose();
                physicsSimulator.Remove(bp.Body);
                physicsSimulator.Remove(bp.Geom);
                bp.Geom = null;
                bp.Body = null;
            }

            // Repeats as above with the hands

            foreach (BodyPart bp in hands)
            {
                bp.Enabled = false;
                bp.Body.enabled = false;
                bp.Geom.CollisionEnabled = false;
                
                bp.Geom.CollisionResponseEnabled = false;
                bp.Body.Dispose();
                bp.Geom.Dispose();
                physicsSimulator.Remove(bp.Body);
                physicsSimulator.Remove(bp.Geom);
                bp.Geom = null;
                bp.Body = null;
            }

            //Removes every pinjoint from the simulator
            foreach (PinJoint pj in jointList)
            {
                pj.Dispose();
                physicsSimulator.Remove(pj);                
            }
            jointList.Clear();
            //Removes every angle limit joint from the simulator
            foreach (AngleLimitJoint alj in angleLimitJointList)
            {
                alj.Dispose();
                physicsSimulator.Remove(alj);
            }
            angleLimitJointList.Clear();
            
            //reinitializes the texture paths, body parts, and recreates the climber.
            initializeTexturePaths();
            initializeBodyParts();
            setStats(height, weight, flex, strength, friction, physicsSimulator, game, initialPosition);            
            
        }


        /// <summary>
        /// Dynamically adjusts angle of knees based on hip angle.
        /// </summary>
        private void adjustKneeJoints()
        {
            /* If the angle of the hip is past horizontal, creates two new knee joints
             * based on the new angle of the hip.
             */
            if((leftThighAngleLimitJoint.Difference >= Math.PI / 2)&&(leftThighAngleLimitJoint.Difference<=Math.PI))
            {
                physicsSimulator.Remove(leftKneeAngleLimitJoint2);
                physicsSimulator.Remove(leftKneeAngleLimitJoint3);
                leftKneeAngleLimitJoint2 = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, leftThighPart.getBody(), leftKneePart.getBody(), -leftThighAngleLimitJoint.Difference / 2f, -leftThighAngleLimitJoint.Difference / 2f);
                leftKneeAngleLimitJoint3 = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, leftKneePart.getBody(), leftCalfPart.getBody(), -leftThighAngleLimitJoint.Difference / 2f, -leftThighAngleLimitJoint.Difference / 2f);
            }
            /* If the angle of the hip is below, creates two new knee joints
             * with default values.
             */
            else if (leftThighAngleLimitJoint.Difference < Math.PI / 2)
            {
                physicsSimulator.Remove(leftKneeAngleLimitJoint2);
                physicsSimulator.Remove(leftKneeAngleLimitJoint3);
                leftKneeAngleLimitJoint2 = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, leftThighPart.getBody(), leftKneePart.getBody(), -1f, 0f);
                leftKneeAngleLimitJoint3 = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, leftKneePart.getBody(), leftCalfPart.getBody(), -1f, 0f);
            }
            /* If the angle of the hip is past horizontal, creates two new knee joints
             * based on the new angle of the hip.
             */
            if ((rightThighAngleLimitJoint.Difference <= (Math.PI / 2f)*-1) && (rightThighAngleLimitJoint.Difference>=(Math.PI)*-1))
            {
                
                physicsSimulator.Remove(rightKneeAngleLimitJoint2);
                physicsSimulator.Remove(rightKneeAngleLimitJoint3);
                rightKneeAngleLimitJoint2 = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, rightThighPart.getBody(), rightKneePart.getBody(), (rightThighAngleLimitJoint.Difference / 2f)*-1, (rightThighAngleLimitJoint.Difference / 2f)*-1);
                rightKneeAngleLimitJoint3 = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, rightKneePart.getBody(), rightCalfPart.getBody(), (rightThighAngleLimitJoint.Difference / 2f)*-1, (rightThighAngleLimitJoint.Difference / 2f)*-1);
            }
            /* If the angle of the hip is below, creates two new knee joints
             * with default values.
             */
            else
            {
                if ((rightKneeAngleLimitJoint2.LowerLimit != 0f) || (rightKneeAngleLimitJoint3.UpperLimit != 1f))
                {
                    physicsSimulator.Remove(rightKneeAngleLimitJoint2);
                    physicsSimulator.Remove(rightKneeAngleLimitJoint3);
                    rightKneeAngleLimitJoint2 = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, rightThighPart.getBody(), rightKneePart.getBody(), 0f, 1f);
                    rightKneeAngleLimitJoint3 = JointFactory.Instance.CreateAngleLimitJoint(physicsSimulator, rightKneePart.getBody(), rightCalfPart.getBody(), 0f, 1f);
                }
            }             
            
        }

        /// <summary>
        /// Checks to see if only feet are in the contact object list.  If so, climber falls.
        /// </summary>
        private void feetCheck()
        {
           
            int feetCount = 0;
            //Turns off collision on feet if they are swinging freely and not selected.
            if ((!leftFootPart.IsHolding) && (!leftFootPart.IsSelected))
                leftFootPart.getGeom().CollisionEnabled = false;
            if ((!rightFootPart.IsHolding) && (!rightFootPart.IsSelected))
                rightFootPart.getGeom().CollisionEnabled = false;
            
            //Increments feet count based on contact object list
            foreach (ContactObject co in contactObjectsList)
            {
                if ((co.contactHand.getName() == "rightFoot") || (co.contactHand.getName() == "leftFoot"))
                {
                    feetCount++;                
                }
            }

            //Lets climber go if he is only holding on with one foot.
            if ((contactObjectsList.Count == 1) && (feetCount == 1))
            {
                foreach (ContactObject co in contactObjectsList)
                {
                    co.contactHand.getBody().IsStatic = false;
                    co.contactHand.getGeom().CollisionEnabled = false;
                    co.contactHand.getBody().ApplyForce(new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -5000));
                    co.contactHand.IsHolding = false;
                    badContacts.Add(co);
                }
                hasLetGo = true;
                
            }

            //Lets climber go if he is only holding on with two feet.
            if ((contactObjectsList.Count == 2) && (feetCount == 2))
            {             
                foreach (ContactObject co in contactObjectsList)
                {
                    co.contactHand.getBody().IsStatic = false;
                    co.contactHand.getGeom().CollisionEnabled = false;
                    co.contactHand.getBody().ApplyForce(new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, -5000));
                    co.contactHand.IsHolding = false;
                    badContacts.Add(co);
                }
                hasLetGo = true;
            }
                //letGo();
            if (badContacts.Count > 0)
            {
                badContacts.Clear();
                contactObjectsList.Clear();
            }

        }

        /// <summary>
        /// Allows the game component to update itself.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        public override void Update(GameTime gameTime)
        {
            
            thisState = Mouse.GetState();
            thisKeyboardState = Keyboard.GetState();
            thisGamepadState = GamePad.GetState(PlayerIndex.One);

            if (!hasLetGo)
                HandleInput(thisState);
            
            lastKeyboardState = thisKeyboardState;


            //Console.WriteLine(wallList.Count);
            
            //Turns on the gravity if gravitytoggle is true, meaning he has grabbed two holds.
            if (gravityToggle)
            {
                if (physicsSimulator.Gravity == new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, 0))
                    physicsSimulator.Gravity = new FarseerGames.FarseerPhysics.Mathematics.Vector2(0, 550);
            }

            //Toggles gravity if he has grabbed two holds
            if (contactObjectsList.Count > 1)
                gravityToggle = true;
                        
            first = false;
         
         
            //Turns off collision on all relevant body parts if falling.
            if ((contactObjectsList.Count == 0) && (gravityToggle) && (floorContact))
            {
                hasLetGo = true;
                rightCalfPart.Geom.CollisionEnabled = true;
                leftCalfPart.Geom.CollisionEnabled = true;
                rightThighPart.Geom.CollisionEnabled = true;
                leftThighPart.Geom.CollisionEnabled = true;
            }

            //checks the grip of each contact object.
            checkGrip(gameTime);
            //checks the feet
            feetCheck();
            //dynamically adjust the knee joints
            adjustKneeJoints();

            lastState = thisState;
            lastKeyboardState = thisKeyboardState;
            lastGamepadState = thisGamepadState;
            lastLeftShoulderButtonPressed = leftShoulderButtonPressed;

            base.Update(gameTime);

        }

        /// <summary>
        /// Allows the game component to draw itself.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        public override void Draw(GameTime gameTime)
        {            
            foreach (BodyPart bp in bodyPartsList)
            {
                if (bp.getTexture() != null)
                    if (!bp.IsSelected)
                        spriteBatch.Draw(bp.getTexture(), new Microsoft.Xna.Framework.Vector2(bp.getGeom().Position.X, bp.getGeom().Position.Y), null, Color.White, bp.getGeom().Rotation, bp.getOrigin(), 1, SpriteEffects.None, 0);
                    else
                        spriteBatch.Draw(bp.getTexture(), new Microsoft.Xna.Framework.Vector2(bp.getGeom().Position.X, bp.getGeom().Position.Y), null, Color.Red, bp.getGeom().Rotation, bp.getOrigin(), 1, SpriteEffects.None, 0);
            }
            
        }
    }
}